//
// Created by chen on 25-5-10.
//

#include "PositionControlLoop.h"
#include <cmath>

PositionControlLoop::PositionControlLoop(float kp, float ki, float kd, float integralLimit, float outputLimit, float dcBusVoltage)
    : ControlLoop(kp, ki, integralLimit, outputLimit, dcBusVoltage),
      lastPositionError(0.0f), positionKd(kd) {}

void PositionControlLoop::execute(float angle) {
    float error = targetValue - actualValue;
    float derivative = error - lastPositionError;
    lastPositionError = error;

    // 执行PI控制并添加微分分量
    float output = piController.compute(targetValue, actualValue) + positionKd * derivative;

    // 位置环输出作为速度环的目标值
    outputD = 0.0f;
    outputQ = output;

    // 执行反Park变换
    float cosTheta = std::cos(angle);
    float sinTheta = std::sin(angle);
    valpha = outputD * cosTheta - outputQ * sinTheta;
    vbeta = outputD * sinTheta + outputQ * cosTheta;
}